#include "project_config.h"

#if (CMD_GAS320_FM == 1)
#include "qelib.h"
#include "gas320_fm_ioc.h"

static qe_const_str usages[] = {
    "fm [options] [[--] args]",
    "fm [options]",
    QE_NULL,
};

static qe_ret action_query_pos(qe_argparse *parser, qe_option *option)
{
    qe_ret ret;
    gas320_fm_pos pos;
    qe_dev *dev = (qe_dev *)qe_option_data(option);
    ret = qe_dev_ioctl(dev, GAS320_FM_IOC_QUERY_POS, &pos);
    if (ret != qe_ok) {
        qe_printf("query pos err:%d\r\n", ret);
    } else {
        qe_printf("query pos %d\r\n", pos.pos);
    }
    return qe_yield;
}

static qe_ret action_move_stop(qe_argparse *parser, qe_option *option)
{
    qe_ret ret;
    qe_dev *dev = (qe_dev *)qe_option_data(option);
    ret = qe_dev_ioctl(dev, GAS320_FM_IOC_MOVE_STOP, QE_NULL);
    if (ret != qe_ok) {
        qe_printf("move stop err:%d\r\n", ret);
    } else {
        qe_printf("move stop ok\r\n");
    }
    return qe_yield;
}

static qe_ret action_move_continue_away(qe_argparse *parser, qe_option *option)
{
    qe_ret ret;
    qe_dev *dev = (qe_dev *)qe_option_data(option);
    ret = qe_dev_ioctl(dev, GAS320_FM_IOC_MOVE_CONT_AWAY, QE_NULL);
    if (ret != qe_ok) {
        qe_printf("move continue away err:%d\r\n", ret);
    } else {
        qe_printf("move continue away ok\r\n");
    }
    return qe_yield;
}

static qe_ret action_move_continue_near(qe_argparse *parser, qe_option *option)
{
    qe_ret ret;
    qe_dev *dev = (qe_dev *)qe_option_data(option);
    ret = qe_dev_ioctl(dev, GAS320_FM_IOC_MOVE_CONT_NEAR, QE_NULL);
    if (ret != qe_ok) {
        qe_printf("move continue near err:%d\r\n", ret);
    } else {
        qe_printf("move continue near ok\r\n");
    }
    return qe_yield;
}

static qe_ret action_move_step_away(qe_argparse *parser, qe_option *option)
{
    qe_ret ret;
    qe_dev *dev = (qe_dev *)qe_option_data(option);
    ret = qe_dev_ioctl(dev, GAS320_FM_IOC_MOVE_STEP_AWAY, QE_NULL);
    if (ret != qe_ok) {
        qe_printf("move step away err:%d\r\n", ret);
    } else {
        qe_printf("move step away ok\r\n");
    }
    return qe_yield;
}

static qe_ret action_move_step_near(qe_argparse *parser, qe_option *option)
{
    qe_ret ret;
    qe_dev *dev = (qe_dev *)qe_option_data(option);
    ret = qe_dev_ioctl(dev, GAS320_FM_IOC_MOVE_STEP_NEAR, QE_NULL);
    if (ret != qe_ok) {
        qe_printf("step near err:%d\r\n", ret);
    } else {
        qe_printf("step near ok\r\n");
    }
    return qe_yield;
}

static void cmd_fm(int argc, const char **argv)
{
    qe_argparse parser;

    qe_dev *dev = qe_dev_find("motor");

    qe_option options[] = {
        QE_OPT_ACTION('p', "pos", "query position",
            (void *)dev, action_query_pos),
        QE_OPT_ACTION('s', "stop", "move stop",
            dev, action_move_stop),
        QE_OPT_ACTION(QE_NULL, "cf", "move continue far away",
            dev, action_move_continue_away),
        QE_OPT_ACTION(QE_NULL, "cn", "move continue near",
            dev, action_move_continue_near),
        QE_OPT_ACTION(QE_NULL, "sf", "move step far away",
            dev, action_move_step_away),
        QE_OPT_ACTION(QE_NULL, "sn", "move step near",
            dev, action_move_step_near),
        QE_OPT_HELP('h', "--help", "gpio help"),
        QE_OPT_END(),
    };

    qe_argp_init(&parser, options, usages,
    		"\nfm command.",
			"\nfm command.");
    
    qe_argp_parse(&parser, argc, (qe_const_str *)argv);
}
QE_SHELL_EXPORT_CMD_EXEC(fm, cmd_fm, focus motor commands);
#endif
